#include "I2Cdev.h"
#include "Wire.h"
#include "MPU6050.h"

#define PIN_B1 A2
#define PIN_B2 7
#define PIN_B3 8
byte b1, b2, b3;

MPU6050 imu;
int16_t ax, ay, az;
int16_t gx, gy ,gz;
float ka=16384, kg=131;

int nextTime = 0;

void setup(){
  pinMode(PIN_B1,INPUT);digitalWrite(PIN_B1,HIGH);
  pinMode(PIN_B2,INPUT);digitalWrite(PIN_B2,HIGH);
  pinMode(PIN_B3,INPUT);digitalWrite(PIN_B3,HIGH);
  Wire.begin();
  Serial.begin(57600);
  Serial.println("Initializing I2C devices...");
  imu.initialize();
  Serial.println("testing mpu6050 connections...");
  Serial.println(imu.testConnection()?"MPU6050 connection successful":"MPU6050 connection fialed");
}

void loop(){
  rate(20);
  imu.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
  getButtonStatus(&b1, &b2, &b3);
  /* 
   * valid message format: "B ax,ay,az,gx,gy,gz E\n" 
   * Begin with "B" and End with "E", datas are divided by a comma,
   * and a line end with \n
   * sent in ASCII
  */
  Serial.print("B");
  Serial.print(ax/ka, 4);Serial.print(",");
  Serial.print(ay/ka, 4);Serial.print(",");
  Serial.print(az/ka, 4);Serial.print(",");
  Serial.print(gx/kg);Serial.print(",");
  Serial.print(gy/kg);Serial.print(",");
  Serial.print(gz/kg);Serial.print(",");
  Serial.print(b1);Serial.print(",");
  Serial.print(b2);Serial.print(",");
  Serial.print(b3);Serial.print("E\n");
  
}

void getButtonStatus(byte *b1_status, byte *b2_status, byte *b3_status ){
  *b1_status = digitalRead(PIN_B1);
  *b2_status = digitalRead(PIN_B2);
  *b3_status = digitalRead(PIN_B3);
}

void rate(byte frequency){
  int duration = 1000/frequency; //to get how long the time gap is in miliseconds
  int nowTime = millis();
  while (nowTime < nextTime){
    nowTime = millis();
    delay(1);
  }
  nextTime += duration;
}

